#include <cstdlib>
#include <iostream>
#include <random>
#include <Eigen/Core>
#include <vector>
#include <particle.h>
#include <pcl/point_types.h>


#define PI 3.14159265359

class Particle_Filter{

public:

    Particle_Filter(int number_of_particles, float x, float y, float vx, float vy, int seed)
    {
        this->generator.seed(seed);
        initParticle_Filter(number_of_particles, x, y, vx, vy);
    }

    ~Particle_Filter() {}

    void printParticlesPositions(std::vector<double> &xs, std::vector<double> &ys, std::vector<double> &ps)
    {
        for(int i = 0; i < this->particles.size(); i++)
        {
            xs.push_back(particles[i].getPositionX());
            ys.push_back(particles[i].getPositionY());
            ps.push_back(particles[i].getProbability());
            //std::cout << particles[i].getPositionX() << " , " <<  particles[i].getPositionY() << std::endl;
            //std::cout << particles[i].getProbability() << std::endl;
        }
    }

    void move_particles(float time)
    {
        for (int r = 0; r < particles.size(); ++r) {
            particles[r].update_particle(time);
        }
    }

    void update_particle_weight(float x, float y, float vx, float vy)
    {
        for (int r = 0; r < particles.size(); ++r) {
            particles[r].determine_particle_weight(x, y, vx, vy);
        }
        this->resample_particles(vx, vy);
    }

    void mean_position(float &x, float &y)
    {
        for (int j = 0; j < particles.size(); ++j) {
            x += particles[j].getPositionX();
            y += particles[j].getPositionY();
        }

        x = x / particles.size();
        y = y / particles.size();
    }


private:

    std::default_random_engine generator;
    std::normal_distribution<float> distribution;
    std::vector<Particle> particles;
    std::vector<Particle> temp_particles;

    void initParticle_Filter(int &number_of_particles, float x, float y, float vx, float vy)
    {
        srandom(time(0));
        if(number_of_particles<=500){
            number_of_particles = 500;
        }
        for(int i=0; i < number_of_particles; i++)
        {
            Particle p(x, y, vx, vy, rand());
            this->particles.push_back(p);
        }
    }

    void resample_particles(float vx, float vy)
    {
        temp_particles.clear();
        int index = int(rand() %  particles.size());
        double beta = 0.0;
        double mw = -1.0;
        for(int s = 0; s < particles.size();s++)
        {
            if(mw < particles[s].getProbability())
            {
                mw = particles[s].getProbability();
            }
        }
        for(int j = 0; j<particles.size(); j++)
        {
            beta += (double)rand()/(double)RAND_MAX * 2.0 * mw;
            while (beta > particles[index].getProbability())
            {
                beta -= particles[index].getProbability();
                index = (index + 1) % particles.size();
            }
            particles[index].setVelocity(vx,vy);
            temp_particles.push_back(particles[index]);
        }
        particles.clear();
        particles = temp_particles;
    }

};
